#ifndef VECTORBOOL_H_INCLUDED
#define VECTORBOOL_H_INCLUDED

#include "Constants.h"
#include "MemoryManager.h"
#include <cstring>

namespace CEngine {

Vector<bool>::Vector(void)
    :   array(NULL),
        size(0),
        capacity(0)
{}

Vector<bool>::Vector(const U64 n)
    :   array(NULL),
        size(n)
{
    capacity = size / 64;
    if((size % 64) != 0) {
        ++capacity;
    }
    array = (U64*)MemoryManager::rawAlloc(capacity * sizeof(U64));

    memset(array, 0, capacity * sizeof(U64));
}

Vector<bool>::Vector(const U64 n, const bool& element)
    :   array(NULL),
        size(n)
{
    capacity = size / 64;
    if((size % 64) != 0) {
        ++capacity;
    }
    array = (U64*)MemoryManager::rawAlloc(capacity * sizeof(U64));

    if(element) {
        memset(array, 255, capacity * sizeof(U64));
    }
    else {
        memset(array, 0, capacity * sizeof(U64));
    }
}

template<class T>
Vector<bool>::Vector(const Iterator<T>& first, const Iterator<T>& last)
    :   array(NULL)
{
    ASSERT(first < last);

    size = last.getPosition() - first.getPosition();

    capacity = size / 64;
    if((size % 64) != 0) {
        ++capacity;
    }
    array = (U64*)MemoryManager::rawAlloc(capacity * sizeof(U64));

    Iterator<T> it = first;
    for (U64 i = 0; it != last; ++it) {
        setBit(i, *it);
        ++i;
    }
}

Vector<bool>::Vector(const Vector& vector)
    :   array(NULL),
        size(vector.size),
        capacity(vector.capacity)
{
    array = (U64*)MemoryManager::rawAlloc(capacity * sizeof(U64));

    memcpy(array, vector.array, capacity * sizeof(U64));
}

Vector<bool>&
Vector<bool>::operator=(const Vector& vector) {
    if(&vector == this) {
        return *this;
    }

    if(vector.size > capacity) {
        if(array) {
            MemoryManager::free(array);
        }
        array = (U64*)MemoryManager::rawAlloc(capacity * sizeof(U64));
        capacity = vector.size;
    }

    memcpy(array, vector.array, capacity * sizeof(U64));

    size = vector.size;

    return *this;
}

Vector<bool>::~Vector(void) {
    if(array) {
        MemoryManager::free((void*)array);
        array = NULL;
    }
}

bool Vector<bool>::front(void) const {
    if(size == 0) {
        defaultLog.log(CEngine::Logging::CONTAINERS, LOG_ERROR, "Accessed element in empty container!\n");
    }

    return getBit(0);
}

bool Vector<bool>::back(void) const {
    if(size == 0) {
        defaultLog.log(CEngine::Logging::CONTAINERS, LOG_ERROR, "Accessed element in empty container!\n");
    }

    return getBit(size - 1);
}

void Vector<bool>::pushBack(const bool& element) {
    U64 temp = capacity * 64;
    if(size == temp) {
        if(array) {
            array = (U64*)MemoryManager::grow(array, capacity * sizeof(U64), capacity * sizeof(U64) * 2);
            capacity *= 2;
        }
        else {
            array = (U64*)MemoryManager::rawAlloc(sizeof(U64) * 2);
            capacity = 2;
        }
    }

    setBit(size++, element);
}

void Vector<bool>::popBack(void) {
    if(size == 0) {
        return;
    }
    --size;
}

void Vector<bool>::insert(const U64 position, const bool& element) {
    U64 temp = capacity * 64;
    if(size == temp) {
        if(array) {
            array = (U64*)MemoryManager::grow(array, capacity * sizeof(U64), capacity * sizeof(U64) * 2);
            capacity *= 2;
        }
        else {
            array = (U64*)MemoryManager::rawAlloc(sizeof(U64) * 2);
            capacity = 2;
        }
    }

    if(position >= size) {
        setBit(size++, element);
        return;
    }

    shiftBits(position, 1, true);

    setBit(position, element);
    ++size;
}

void Vector<bool>::insert(const U64 position, const U64 n, const bool& element) {
    U64 temp = capacity * 64;
    if((size + n) >= temp) {
        if(array) {
            U64 oldCapacity = capacity;
            for (; temp < size + n;) {
                temp *= 2;
                capacity *= 2;
            }

            array = (U64*)MemoryManager::grow(array, oldCapacity * sizeof(U64), capacity * sizeof(U64));
        }
        else {
            capacity = 2;
            temp = 128;
            for (; temp < size + n;) {
                temp *= 2;
                capacity *= 2;
            }

            array = (U64*)MemoryManager::rawAlloc(sizeof(U64) * capacity);
        }
    }

    if(position >= size) {
        for (U64 i = 0; i < n; ++i) {
            setBit(size++, element);
        }
        return;
    }

    shiftBits(position, n, true);

    for (U64 i = position; i < (position + n); ++i) {
        setBit(i, element);
    }
    size += n;
}

void Vector<bool>::erase(const U64 position) {
    if(size == 0) {
        return;
    }

    if(position >= size) {
        --size;
        return;
    }

    shiftBits(position, 1, false);

    --size;
}

void Vector<bool>::erase(const U64 start, const U64 e) {
    U64 end = e - 1;
    if(size == 0 || start > end) {
        return;
    }
    if(end >= size) {
        end = size;
        size -= end - start;
    }

    U64 difference = end - start;
    shiftBits(start, difference, false);
    size -= difference;
}

void Vector<bool>::resize(const U64 n) {
    if(n <= size) {
        size = n;
        return;
    }
    U64 temp = n / 64;
    if((n % 64) != 0) {
        ++temp;
    }
    if(temp > capacity) {
        if(array) {
            array = (U64*)MemoryManager::grow(array, capacity * sizeof(U64), temp * sizeof(U64));
        }
        else {
            array = (U64*)MemoryManager::rawAlloc(temp * sizeof(U64));
        }
        capacity = temp;
    }

    memset(array + size, 0, n - size);

    size = n;
}

void Vector<bool>::resize(const U64 n, const bool& element) {
    if(n <= size) {
        size = n;
        return;
    }
    U64 temp = n / 64;
    if((n % 64) != 0) {
        ++temp;
    }
    if(temp > capacity) {
        if(array) {
            array = (U64*)MemoryManager::grow(array, capacity * sizeof(U64), temp * sizeof(U64));
        }
        else {
            array = (U64*)MemoryManager::rawAlloc(temp * sizeof(U64));
        }
        capacity = temp;
    }

    memset(array + size, element, n - size);

    size = n;
}

void Vector<bool>::reserve(const U64 n) {
    U64 temp = n / 64;
    if((n % 64) != 0) {
        ++temp;
    }
    if(temp > capacity) {
        if(array) {
            array = (U64*)MemoryManager::grow(array, capacity * sizeof(U64), temp * sizeof(U64));
        }
        else {
            array = (U64*)MemoryManager::rawAlloc(temp * sizeof(U64));
        }
        capacity = temp;
    }
}

void Vector<bool>::shrink(const U64 n) {
    U64 temp = n / 64;
    if((n % 64) != 0) {
        ++temp;
    }
    if(temp < capacity){
        if(array) {
            U64* tempPointer = array;
            array = (U64*)MemoryManager::rawAlloc(temp * sizeof(U64));
            memcpy(array, tempPointer, temp * sizeof(U64));
        }
        else {
            array = (U64*)MemoryManager::rawAlloc(temp * sizeof(U64));
        }
        capacity = temp;
        temp = capacity * 64;
        if(size > capacity) {
            size = capacity;
        }
    }
}

void Vector<bool>::clear(void) {
    size = 0;
}

inline
bool Vector<bool>::isEmpty(void) const {
    if(size == 0) {
        return true;
    }
    return false;
}

inline
U64 Vector<bool>::getSize(void) const {
    return size;
}

inline
U64 Vector<bool>::getCapacity(void) const {
    return capacity * 64;
}

inline
U64* Vector<bool>::getRawArray(void) const {
    return array;
}

inline bool
Vector<bool>::getBit(const U64 p) const{
    U64 position = p;
    if(p >= size) {
        position = size - 1;
    }
    U64 block = position / 64;
    U8 bit = position % 64;
    return (bool)(array[block] & ((U64)1 << bit));
}

inline void
Vector<bool>::setBit(const U64 p, const bool set) {
    U64 position = p;
    if(p >= size) {
        position = size - 1;
    }
    U64 block = position / 64;
    U8 bit = position % 64;
    if(set) {
        array[block] |= ((U64)1 << bit);
    }
    else {
        array[block] &= ~((U64)1 << bit);
    }
}

void
Vector<bool>::shiftBits(const U64 position, U64 sizeOfShift, const bool shiftRight) {
    ASSERT(sizeOfShift > 0 && sizeOfShift < 64);
    if(shiftRight) {
        ///Calculates the starting and ending blocks (starting at the end of the array and working back).
        U64 finalBlock = position / 64;
        U64 currentBlock = ((size - 1) + sizeOfShift) / 64;
        ///Number of bits to be copied across blocks per cycle, during the final cycle.
        U8 bitsToCopy = ((size - 1) + sizeOfShift) % 64;
        U8 finalCopy = position % 64;

        ///Sets the masks used to grab and set bits in blocks.
        U64 copyMask = UINT64_MAX << ((U64)64 - bitsToCopy);
        U64 pasteMask = ~(UINT64_MAX << bitsToCopy);

        while(currentBlock != finalBlock) {
            ///copy bits from next block.
            U64 temp = array[currentBlock - 1] & copyMask;
            ///shift bits into position for pasting.
            temp = temp >> ((U64)64 - bitsToCopy);
            ///paste bits into current block, and shift next block over for next cycle preparation.
            array[currentBlock] = (array[currentBlock] & ~pasteMask) | temp;
            array[currentBlock - 1] = array[currentBlock - 1] >> ((U64)64 - bitsToCopy);

            --currentBlock;
        }
        ///Set masks for final block.
        copyMask = UINT64_MAX << ((U64)64 - finalCopy);
        pasteMask = ~(UINT64_MAX << finalCopy);
        ///Copy and shift bits.
        U64 temp = array[currentBlock] & copyMask;
        temp = temp << sizeOfShift;
        ///Paste bits.
        array[currentBlock] = (array[currentBlock] & ~pasteMask) | temp;
    }
    else {
        ASSERT_MESSAGE(false, shiftBits needs to be finished first!);

        ///Calculates the starting and ending blocks (starting at the end of the array and working back).
        U64 finalBlock = ((size - 1) + sizeOfShift) / 64;
        U64 currentBlock = position / 64;
        ///Number of bits to be copied across blocks per cycle, during the final cycle.
        U8 bitsToCopy = 64 - (position % 64);
        U8 finalCopy = (((size - 1) + sizeOfShift) % 64);

        ///Sets the masks used to grab and set bits in blocks.
        U64 copyMask = ~(UINT64_MAX << bitsToCopy);
        U64 pasteMask = UINT64_MAX << ((U64)64 - bitsToCopy);

        while(currentBlock != finalBlock) {
            ///copy bits from next block.
            U64 temp = array[currentBlock + 1] & copyMask;
            ///shift bits into position for pasting.
            temp = temp << ((U64)64 - bitsToCopy);
            ///paste bits into current block, and shift next block over for next cycle preparation.
            array[currentBlock] = (array[currentBlock] & ~pasteMask) | temp;
            array[currentBlock + 1] = array[currentBlock + 1] << ((U64)64 - bitsToCopy);

            ++currentBlock;
        }
        ///Set masks for final block.
        copyMask = ~(UINT64_MAX << finalCopy);
        pasteMask = UINT64_MAX << ((U64)64 - finalCopy);
        ///Copy and shift bits.
        U64 temp = array[currentBlock] & copyMask;
        temp = temp << sizeOfShift;
        ///Paste bits.
        array[currentBlock] = (array[currentBlock] & ~pasteMask) | temp;
    }
}

}

#endif // VECTORBOOL_H_INCLUDED
